#include "octree_rts.hh"

OctreeRts::OctreeRts (unsigned int size)
  : Octree (size)
{
  map_is_loaded = false;

  // la size donnee n'est pas valide
  if (size != 512 && size != 1024 &&
      size != 2048 && size != 4096)
    {
      std::cerr << "World size must be 512, 1024, 2048 or 4096" << std::endl;
      assert (false);
    }

  int curr = 0;
  int cpt = 1;

  // on calcule NB_OVERLAP et NB_NODE_256
  while (size != 512)
    {
      curr += cpt;
      cpt *= 4;
      size /= 2;
    }

  NB_OVERLAP = cpt + curr;
  NB_NODE_256 = cpt * 4;

  // on initialise les tableaux de noeuds et chevauchements
  tab_256_ = new NodeRts[NB_NODE_256];

  overlap_ = new List<Object*>*[NB_OVERLAP];
  for (int i = 0; i < NB_OVERLAP; ++i)
    overlap_[i] = 0;
}

OctreeRts::~OctreeRts ()
{
  // on detruit le tableau de noeuds
  delete[] tab_256_;

  List<Object*>* tmp = 0;
  List<Object*>* pred = 0;

  // pour chacune des liste d'objet du tableau de chevauchements
  // on detruit tous les elements
  for (int i = 0; i < NB_OVERLAP; ++i)
    {
      tmp = overlap_[i];

      while (tmp != 0)
	{
	  pred = tmp;
	  if (pred->elt != 0)
	    delete pred->elt; // on detruit l'element
	  tmp = tmp->next;
	  delete pred; // on detruit le noeud de liste qui le contient
	}
    }

  // puis on detruit le tableau lui meme
  delete[] overlap_;
}

int
OctreeRts::calc_nb_node_pred (int index)
{
  int cpt = 1;

  // on recupere le nombre de fils de l'etage avant index
  while (index != 0)
    {
      index -= cpt;
      cpt *= 4;
    }

  return (cpt / 4);
}

int
OctreeRts::index_get (int step)
{
  int pos = 0;
  int cpt = 1;

  // on recupere l'index de l'etage demande dans step
  while ((int) size_ > step)
    {
      pos += cpt;
      cpt *= 4;
      step *= 2;
    }

  return pos;
}

void
OctreeRts::add_obj (Object* obj)
{

  int nb_node = size_ / 256;

  Vector3f pos = obj->pos_get ();
  int block_id = ((int) pos (0)) / 256 + (((int) pos (1)) / 256) * nb_node;

  Vector3f size = obj->size_get ();
  int id_front = ((int) (pos (0) + size (0))) / 256 + (((int) pos (1)) / 256) * nb_node;
  int id_back = ((int) (pos (0) - size (0))) / 256 + (((int) pos (1)) / 256) * nb_node;
  int id_right = ((int) pos (0)) / 256 + (((int) (pos (1) + size (1))) / 256) * nb_node;
  int id_left = ((int) pos (0)) / 256 + (((int) (pos (1) - size (1))) / 256) * nb_node;

  int i = NB_OVERLAP;
  int j = calc_nb_node_pred (i);

  // tant que je ne rentre pas dans le noeud courant, j'essaye avec mon pere
  while (id_front != block_id ||
	 id_back != block_id ||
	 id_right != block_id ||
	 id_left != block_id)
    {
      id_front = (id_front % nb_node) / 2 + (id_front / nb_node) / 2;
      id_back = (id_back % nb_node) / 2 + (id_back / nb_node) / 2;
      id_left = (id_left % nb_node) / 2 + (id_left / nb_node) / 2;
      id_right = (id_right % nb_node) / 2 + (id_right / nb_node) / 2;
      block_id = (block_id % nb_node) / 2 + (block_id / nb_node) / 2;
      i -= j;
      j /= 4;
    }

  if (i == NB_OVERLAP) // je rentre dans un noeud de taille 256
    tab_256_[block_id].add_obj (obj);
  else // je suis dans un chevauchement au dessus de 256
    {
      i += block_id;
      List<Object*>* tmp = 0;

      // on ajoute l'objet en tete de liste
      tmp = new List<Object*>;
      tmp->elt = obj;
      tmp->next = overlap_[i];
      overlap_[i] = tmp;
    }
}

bool
OctreeRts::delete_obj (Object* obj)
{
  int nb_node = size_ / 256;

  Vector3f pos = obj->pos_get ();
  int block_id = ((int) pos (0)) / 256 + (((int) pos (1)) / 256) * nb_node;

  Vector3f size = obj->size_get ();
  int id_front = ((int) (pos (0) + size (0))) / 256 + (((int) pos (1)) / 256) * nb_node;
  int id_back = ((int) (pos (0) - size (0))) / 256 + (((int) pos (1)) / 256) * nb_node;
  int id_right = ((int) pos (0)) / 256 + (((int) (pos (1) + size (1))) / 256) * nb_node;
  int id_left = ((int) pos (0)) / 256 + (((int) (pos (1) - size (1))) / 256) * nb_node;

  int i = NB_OVERLAP;
  int j = calc_nb_node_pred (i);

  // tant que je ne rentre pas dans le noeud courant, j'essaye avec mon pere
  while (id_front != block_id ||
	 id_back != block_id ||
	 id_right != block_id ||
	 id_left != block_id)
    {
      id_front = (id_front % nb_node) / 2 + (id_front / nb_node) / 2;
      id_back = (id_back % nb_node) / 2 + (id_back / nb_node) / 2;
      id_left = (id_left % nb_node) / 2 + (id_left / nb_node) / 2;
      id_right = (id_right % nb_node) / 2 + (id_right / nb_node) / 2;
      block_id = (block_id % nb_node) / 2 + (block_id / nb_node) / 2;
      i -= j;
      j /= 4;
    }

  if (i == NB_OVERLAP) // je suis dans un noeud de taille 256
    return tab_256_[block_id].delete_obj (obj);

  i += block_id;

  // je recupere la liste d'objet du noeud courant
  List<Object*>* tmp = overlap_[i];
  List<Object*>* pred = tmp;

  // je cherche l'objet demande
  while (tmp != 0 && tmp->elt != obj)
    {
      pred = tmp;
      tmp = tmp->next;
    }

  if (tmp != 0) // il existe
    {
      if (tmp == overlap_[i]) // je suis en tete de liste
	overlap_[i] = tmp->next;
      else
	pred->next = tmp->next; // ou n'importe ou ailleurs

      delete tmp; // Suppression reussie
      return true;
    }

  // ou il existe pas
  return false;
}

void
OctreeRts::accept (PhysicFunctor* phy)
{

  int nb_node = size_ / 256;
  Object* obj = phy->object_get ();

  // on delete l'objet de l'octree
  bool b = delete_obj (obj);
  if (!b)
    return;

  Vector3f pos = obj->pos_get ();
  Vector3f size = obj->size_get ();

  // on recupere les 4 angles de l'objet
  //   *     *
  //      -
  //   *     *
  int y1 = ((int) (pos (1) - size (1))) / 256;
  int y2 = ((int) (pos (1) + size (1))) / 256;
  int x1 = ((int) (pos (0) - size (0))) / 256;
  int x2 = ((int) (pos (0) + size (0))) / 256;

  // traitement des noeuds de taille 256 qui contiennent au moins un bout de l'objet
  for (int i = x1; i <= x2; ++i)
    for (int j = y1; j <= y2; ++j)
      tab_256_[i + j * nb_node].accept (phy);
  // recuperation de l'index du premier noeud de taille 512
  int current = index_get (512);
  int cpt = calc_nb_node_pred (current);
  
  // on remonte l'arbre jusqu'a la racine
  while (current != 0)
    {
      nb_node /= 2;
      y1 /= 2;
      y2 /= 2;
      x1 /= 2;
      x2 /= 2;

      for (int i = x1; i <= x2; ++i)
	for (int j = y1; j <= y2; ++j)
	  (*phy) (overlap_[current + i + j * nb_node]);

      current -= cpt;
      cpt /= 4;
    }

  // on traite le chevauchement du tout premier noeud
  (*phy) (overlap_[0]);

  // on remet l'objet mis a jour dans l'octree
  add_obj (obj);
}

void
OctreeRts::accept (DisplayFunctor* disp)
{
  for (int i = 0; i < NB_OVERLAP; ++i)
    (*disp) (overlap_[i]);

  for (int i = 0; i < NB_NODE_256; ++i)
    tab_256_[i].accept (disp);
}

void
OctreeRts::accept (MapFunctor* mf)
{

  for (int i = 0; i < NB_NODE_256; ++i)
    tab_256_[i].accept (mf);
}

void
OctreeRts::insert_map (Map* map)
{
  int nb_node = size_ / 256;

  for (int i = 0; i < NB_NODE_256 ; ++i)
    tab_256_[i].insert_map (map, (i % nb_node) * 256, (i / nb_node) * 256);

  map_is_loaded = true;
}

void
OctreeRts::accept (GetObjectFunctor* objf, Vector2d* pos)
{
  Vector2d block_256 = (*pos) / 2;
  int nb_node = size_ / 256;

  Vector2d* tmp = pos;
  (*tmp) %= 2;

  tab_256_[block_256 (0) + block_256 (1) * nb_node].accept (objf, tmp);
}
